//
// Created by jwhww on 25-11-11.
//

#include "AllHeadFile.h"

//-------------------------------控制相关变量初始化-------------------------------------
float readyfor_PID_GM6020[8] = {0.0f};     //遥控器数据预处理缓冲区
float readyfor_PID_M3508[8] = {0.0f};
int16_t readyfor_Load_GM6020[8] = {0};   //准备发报区
int16_t readyfor_Load_M3508[8] = {0};
int16_t readyfor_Load_M2006[8] = {0};
int16_t readyfor_Load_CBoard[3] = {0};
float LPS_lastValue_temp[8] = {0.0f};  //遥控器低通滤波所需
static float angle_of_my[4] = {-1.f, -1.f, -1.f, -1.f};    //目标角度
//-------------------------------控制相关变量初始化-------------------------------------
//-------------------------------远程控制can相关变量初始化-------------------------------
//Tx相关
CAN_TxHeaderTypeDef TxHeader_to_SMcomputer; //向小电脑发报
uint32_t Tx_mailbox_toSMcomputer = 0; //发送邮箱
uint8_t can_send_content_SMcomputer[8] = {0};           // CAN发送数据内容
//Rx相关
SMcomputer_InfoTypedef SMcomputer_Info = {0};
//-------------------------------远程控制can相关变量初始化-------------------------------

//------------------------------------------------------------------------------------------------------
// 函数简介         遥控器低通滤波
// 使用示例         LPS_filter()
// 参数介绍         a               低通滤波系数,越大越尖锐，越小越迟钝，常见0.02~0.2或0.5
// 返回参数         无
// 备注信息         仅使用遥控器控制云台
//------------------------------------------------------------------------------------------------------
static float LPS_filter(const float a, const float new_value, const float last_value)
{
        //低通滤波
        return a * new_value + (1.f - a) * last_value;
}

//------------------------------------------------------------------------------------------------------
// 函数简介         最短路径归中
// 使用示例         diff_shortestPathEnter()
// 参数介绍         diff            角度差
// 返回参数         无
// 备注信息         仅使用遥控器控制云台
//------------------------------------------------------------------------------------------------------
static float diff_shortestPathEnter(float diff) {
    return diff > 4096.f ? diff - 8192.f : diff < -4096.f ? diff + 8192.f : diff;
}
//------------------------------------------------------------------------------------------------------
// 函数简介         遥控器控制
// 使用示例         Remote_Control()
// 参数介绍
// 返回参数         无
// 备注信息         仅使用遥控器控制云台
//------------------------------------------------------------------------------------------------------
void NDJ6_Remote_Control(void)
{
//底座1号电机
    if (dbus_ctrl_data.channel.s1 == 1) {   //单速度环
        readyfor_PID_GM6020[0] = -((float)dbus_ctrl_data.channel.CH[0] - 1024.f) * 0.05f; // 0.2 = 132 / (1684 - 1024)  速度换算
        //                                        前馈项                                                  pid项
        readyfor_Load_GM6020[0] = (int16_t)( -(((float)dbus_ctrl_data.channel.CH[0] - 1024.f) * 15.f) + (float)JJ_motor_speed_closeCircle((int16_t)readyfor_PID_GM6020[0], Motor_Info_GM6020, MOTOR_1));
    }
    if (dbus_ctrl_data.channel.s1 == 2) {   //位置串速度
        if (angle_of_my[0] == -1) angle_of_my[0] = (float)Motor_Info_GM6020[0].motor_angle;
        //死区
        float temp_ch0 = (float)dbus_ctrl_data.channel.CH[0];
        if (temp_ch0 >= 1019.f && temp_ch0 <= 1029.f) temp_ch0 = 1024.f;
        //位置环预处理
        angle_of_my[0] -= (temp_ch0 - 1024.f) * 0.02f;
        if (angle_of_my[0] < 0) angle_of_my[0] += 8192;
        if (angle_of_my[0] > 8191) angle_of_my[0] -= 8192;
        readyfor_PID_GM6020[0] = angle_of_my[0];
				  //前馈PID计算
        readyfor_Load_GM6020[0] = (int16_t)(-(((float)dbus_ctrl_data.channel.CH[0] - 1024.f) * 1.f)
                                + (float)JJ_motor_P_S_closeCircle((int16_t)readyfor_PID_GM6020[0], Motor_Info_GM6020, MOTOR_1));
        //低通滤波
        readyfor_Load_GM6020[0] = (int16_t)LPS_filter(0.6f,readyfor_Load_GM6020[0], LPS_lastValue_temp[0]);
        LPS_lastValue_temp[0] = readyfor_Load_GM6020[0];    //更新上一次值
    }
//脑袋2号电机（哨兵头上是2号不是3号）
    if (dbus_ctrl_data.channel.s2 == 1) {   //单速度环
        //死区
         float temp_ch1 = (float)dbus_ctrl_data.channel.CH[1];
         if (temp_ch1 >= 1019.f && temp_ch1 <= 1029.f) temp_ch1 = 1024.f;
        //换算
        readyfor_PID_GM6020[1] = -(temp_ch1 - 1024.f) * (0.05f);  // 0.2 = 132 / (1684 - 1024)
        readyfor_Load_GM6020[1] = (int16_t)(-((temp_ch1 - 1024.f) * 0.f) + (float)JJ_motor_speed_closeCircle((int16_t)readyfor_PID_GM6020[1], Motor_Info_GM6020, MOTOR_2));
    }
    if (dbus_ctrl_data.channel.s2 == 2) {   //位置串速度_OK
        //死区
        if (angle_of_my[1] == -1) angle_of_my[1] = (float)Motor_Info_GM6020[1].motor_angle;
        float temp_ch1 = (float)dbus_ctrl_data.channel.CH[1];
        if (temp_ch1 >= 1019.f && temp_ch1 <= 1029.f) temp_ch1 = 1024.f;

        angle_of_my[1] += (temp_ch1 - 1024.f) * 0.02f;
        angle_of_my[1] = clip_float(angle_of_my[1], 700, 1700); //脑袋电机物理限位：Pitch轴编码器 560~1855
        readyfor_PID_GM6020[1] = angle_of_my[1];
        //固定角度
        readyfor_Load_GM6020[1] = JJ_motor_P_S_closeCircle((int16_t)readyfor_PID_GM6020[1], Motor_Info_GM6020, MOTOR_2);
        //低通滤波
        readyfor_Load_GM6020[1] = (int16_t)LPS_filter(0.7f, readyfor_Load_GM6020[1], LPS_lastValue_temp[1]);
        LPS_lastValue_temp[1] = readyfor_Load_GM6020[1];
    }
//装单轮盘电机(设置最大力矩为最大可达力矩的0.5倍）
    //单速度环
    //堵转检测
    JJ_motor_roulette_blocking_meter(Motor_Info_M3508, MOTOR_1);    //使用SPEED环参数（10.f, 0.5f, 0.f）
    if (Motor_Info_M3508[MOTOR_1].blocking_status != NOTBLOCKING && Motor_Info_M3508[MOTOR_1].blocking_status != BLOCKING_DOUBTFUL) {
        //堵转状态
        readyfor_Load_M3508[0] = Motor_Info_M3508[MOTOR_1].blocking_data.data_back_reverse;
    } else {
        //正常状态
        readyfor_PID_M3508[0] = -((float)dbus_ctrl_data.channel.CH[3] - 1024.f) * (SPEED_MAX_M3508/660.f);  // 0.71 = 469 / (1684 - 1024)
        readyfor_Load_M3508[0] = JJ_motor_speed_closeCircle((int16_t)readyfor_PID_M3508[0], Motor_Info_M3508, MOTOR_1);
    }

//GM6020装载
    JJ_motor_data_load(MOTOR_1, readyfor_Load_GM6020[0]);
    JJ_motor_data_load(MOTOR_2, readyfor_Load_GM6020[1]);
    //JJ_motor_data_load(MOTOR_3, readyToload_GM6020[2]);
    // JJ_motor_data_load(MOTOR_4, tmp_output[3]);
    //GM6020发报
    JJ_motor_ctrl(&hcan2, MOTOR_TYPE_GM6020, MOTOR_GROUP_1234_CURRENT);

//M3508装载
    JJ_motor_data_refresh();
    JJ_motor_data_load(MOTOR_1, readyfor_Load_M3508[0]);
    //M3508发报
    JJ_motor_ctrl(&hcan2, MOTOR_TYPE_M3508, MOTOR_GROUP_1234_CURRENT);

//云台控制底盘
    //云台朝向恒向前解算
    static float y, x ,theta = 0;
    y = ((float)dbus_ctrl_data.channel.CH[3] - 1024.f);
    x = ((float)dbus_ctrl_data.channel.CH[2] - 1024.f);
    theta = ENCODER_TO_RADIAN((float)Motor_Info_GM6020[0].motor_angle - 6808.f);
    readyfor_Load_CBoard[0] = (int16_t)(x * cos((double)theta) - y * sin((double)theta));
    readyfor_Load_CBoard[1] = (int16_t)(y * cos((double)theta) + x * sin((double)theta));
    readyfor_Load_CBoard[2] = (int16_t)((float)dbus_ctrl_data.channel.rolling_wheel - 1024.f);
    CBoard_connect_Load(readyfor_Load_CBoard[0],readyfor_Load_CBoard[1],readyfor_Load_CBoard[2],100);
    CBoard_connect_SendtoDOWN(&hcan1);

}

uint8_t state_SMcomputer = 0;   //当收到过报文后
//------------------------------------------------------------------------------------------------------
// 函数简介         小电脑控制云台
// 使用示例         SMcomputer_Remote_Control()
// 参数介绍
// 返回参数         无
// 备注信息         仅使用小电脑控制云台
//------------------------------------------------------------------------------------------------------
void SMcomputer_Remote_Control(void)
{
    //底座1号电机
    if (SMcomputer_Info.can_id == 0xfff) {  //绝对角度值（编码器）
        if (angle_of_my[0] == -1) angle_of_my[0] = (float)Motor_Info_GM6020[0].motor_angle;
        angle_of_my[0] = RADIAN_TO_ENCODER(SMcomputer_Info.angle_yaw); // 后续根据can收到的STDID来区分收到的小电脑角度报文是增量式还是绝对角度值（目前是绝对角度值）
        //位置环预处理
        if (angle_of_my[0] < 0) angle_of_my[0] += 8192;
        if (angle_of_my[0] > 8191) angle_of_my[0] -= 8192;
        readyfor_PID_GM6020[0] = angle_of_my[0];
        //前馈PID计算
        readyfor_Load_GM6020[0] = JJ_motor_P_S_closeCircle((int16_t)readyfor_PID_GM6020[0], Motor_Info_GM6020, MOTOR_1);
        //低通滤波
        readyfor_Load_GM6020[0] = (int16_t)LPS_filter(0.6f,readyfor_Load_GM6020[0], LPS_lastValue_temp[0]);
        LPS_lastValue_temp[0] = readyfor_Load_GM6020[0];    //更新上一次值

        state_SMcomputer = 1;
    }
    if (SMcomputer_Info.can_id == 0x124) {  //增量角度值(收报是弧度制）
        float diff_angle = RADIAN_TO_ENCODER(SMcomputer_Info.angle_yaw);
        diff_angle = clip_float(diff_angle, -660.f, 660.f); //保持和遥控器定义域一致
        diff_angle = diff_shortestPathEnter(diff_angle) ;
        if (angle_of_my[0] == -1) angle_of_my[0] = (float)Motor_Info_GM6020[0].motor_angle;
        //位置环预处理
        angle_of_my[0] += diff_angle * 0.02f;   //缩放系数0.02f
        if (angle_of_my[0] < 0) angle_of_my[0] += 8192;
        if (angle_of_my[0] > 8191) angle_of_my[0] -= 8192;
        readyfor_PID_GM6020[0] = angle_of_my[0];
        //PID计算
        readyfor_Load_GM6020[0] = JJ_motor_P_S_closeCircle((int16_t)readyfor_PID_GM6020[0], Motor_Info_GM6020, MOTOR_1);
        //低通滤波
        readyfor_Load_GM6020[0] = (int16_t)LPS_filter(0.6f,readyfor_Load_GM6020[0], LPS_lastValue_temp[0]);
        LPS_lastValue_temp[0] = readyfor_Load_GM6020[0];    //更新上一次值

        state_SMcomputer = 1;
    }
    if (SMcomputer_Info.can_id == 0x123) {  //速度控制
        readyfor_PID_GM6020[0] = SMcomputer_Info.speed_yaw;
        readyfor_Load_GM6020[0] = JJ_motor_P_S_closeCircle((int16_t)readyfor_PID_GM6020[0], Motor_Info_GM6020, MOTOR_1);
        //低通滤波
        readyfor_Load_GM6020[0] = (int16_t)LPS_filter(0.6f,readyfor_Load_GM6020[0], LPS_lastValue_temp[0]);
        LPS_lastValue_temp[0] = readyfor_Load_GM6020[0];    //更新上一次值

        state_SMcomputer = 1;
    }

    //云台2号电机
    if (SMcomputer_Info.can_id == 0xfff) {  //绝对角度值
        if (angle_of_my[1] == -1) angle_of_my[1] = (float)Motor_Info_GM6020[1].motor_angle;
        angle_of_my[1] = RADIAN_TO_ENCODER(SMcomputer_Info.angle_pitch);
        angle_of_my[1] = clip_float(angle_of_my[1], 700, 1700); //脑袋电机物理限位：Pitch轴编码器 560~1855
        readyfor_PID_GM6020[1] = angle_of_my[1];
        //PID计算
        readyfor_Load_GM6020[1] = JJ_motor_P_S_closeCircle((int16_t)readyfor_PID_GM6020[1], Motor_Info_GM6020, MOTOR_2);
        //低通滤波
        readyfor_Load_GM6020[1] = (int16_t)LPS_filter(0.7f, readyfor_Load_GM6020[1], LPS_lastValue_temp[1]);
        LPS_lastValue_temp[1] = readyfor_Load_GM6020[1];

        state_SMcomputer = 1;
    }
    if (SMcomputer_Info.can_id == 0x124) {  //增量角度值(收报是弧度制）
        float diff_angle = RADIAN_TO_ENCODER(SMcomputer_Info.angle_pitch);
        diff_angle = clip_float(diff_angle, -660.f, 660.f); //保持和遥控器定义域一致
        diff_angle = diff_shortestPathEnter(diff_angle);
        if (angle_of_my[1] == -1) angle_of_my[1] = (float)Motor_Info_GM6020[1].motor_angle;
        //位置环预处理
        angle_of_my[1] += diff_angle * 0.02f;   //缩放系数0.02f
        angle_of_my[1] = clip_float(angle_of_my[1], 700, 1700); //脑袋电机物理限位：Pitch轴编码器 560~1855
        readyfor_PID_GM6020[1] = angle_of_my[1];
        //PID计算
        readyfor_Load_GM6020[1] = JJ_motor_P_S_closeCircle((int16_t)readyfor_PID_GM6020[1], Motor_Info_GM6020, MOTOR_2);
        //低通滤波
        readyfor_Load_GM6020[1] = (int16_t)LPS_filter(0.7f, readyfor_Load_GM6020[1], LPS_lastValue_temp[1]);
        LPS_lastValue_temp[1] = readyfor_Load_GM6020[1];

        state_SMcomputer = 1;
    }

    if (state_SMcomputer == 1) {
        //装载
        //GM6020装载
        JJ_motor_data_load(MOTOR_1, readyfor_Load_GM6020[0]);
        JJ_motor_data_load(MOTOR_2, readyfor_Load_GM6020[1]);
        //JJ_motor_data_load(MOTOR_3, readyToload_GM6020[2]);
        // JJ_motor_data_load(MOTOR_4, tmp_output[3]);
        //GM6020发报
        JJ_motor_ctrl(&hcan2, MOTOR_TYPE_GM6020, MOTOR_GROUP_1234_CURRENT);

    }
}

//------------------------------------------------------------------------------------------------------
// 函数简介     小电脑沟通初始化
// 使用示例     small_computer_connect_Init();
// 输入参数
// 返回参数     无
// 备注信息     无
//------------------------------------------------------------------------------------------------------
void small_computer_connect_Init(void)
{
    // 初始化CAN
    TxHeader_to_SMcomputer.IDE = CAN_ID_STD;    //STDID在发报的时候才设置
    TxHeader_to_SMcomputer.RTR = CAN_RTR_DATA;
    TxHeader_to_SMcomputer.ExtId = 0x00000000;
    TxHeader_to_SMcomputer.DLC = 8;
    TxHeader_to_SMcomputer.TransmitGlobalTime = DISABLE;
}

//------------------------------------------------------------------------------------------------------
// 函数简介     小电脑沟通-收报
// 使用示例     small_computer_connect_Receive(&hcan1);
// 输入参数     hcan                    CAN句柄
// 返回参数     无
// 备注信息     无
//------------------------------------------------------------------------------------------------------
void small_computer_connect_Receive(const uint8_t* rx_data) {
    // 从CAN接收数据
    // 解析接收数据
    SMcomputer_Info.can_id = RxHeader_CAN1.StdId;
    if (RxHeader_CAN1.StdId == 0x123) { //速度控制
        uint8_t *p = (uint8_t*)&SMcomputer_Info.speed_yaw;  //爱来自GPT
        p[0] = rx_data[0];
        p[1] = rx_data[1];
        p[2] = rx_data[2];
        p[3] = rx_data[3];
        p = (uint8_t*)&SMcomputer_Info.speed_pitch;
        p[0] = rx_data[4];
        p[1] = rx_data[5];
        p[2] = rx_data[6];
        p[3] = rx_data[7];
    }
    else if (RxHeader_CAN1.StdId == 0x124){ //角度控制
        uint8_t *p = (uint8_t*)&SMcomputer_Info.angle_yaw;
        p[0] = rx_data[0];
        p[1] = rx_data[1];
        p[2] = rx_data[2];
        p[3] = rx_data[3];
        p = (uint8_t*)&SMcomputer_Info.angle_pitch;
        p[0] = rx_data[4];
        p[1] = rx_data[5];
        p[2] = rx_data[6];
        p[3] = rx_data[7];
    }

    static uint32_t Timestamp, lastTimestamp = 0;
    lastTimestamp = Timestamp;
    Timestamp = HAL_GetTick();
    if (Timestamp - lastTimestamp >= 100) {
        HAL_GPIO_TogglePin(LED_Red_GPIO_Port, LED_Red_Pin); //小电脑接收数据后，红灯闪烁一下, 配合绿灯表现为橙灯闪烁
    }


}

//------------------------------------------------------------------------------------------------------
// 函数简介     小电脑沟通-报文装载
// 使用示例     small_computer_connect_Load();
// 输入参数     data_yaw            yaw轴数据，可以是角度或速度
// 输入参数     data_pitch          pitch轴数据，可以是角度或速度
// 返回参数     无
// 备注信息     小端序;小电脑要求是float
//------------------------------------------------------------------------------------------------------
void small_computer_connect_Load(float data_yaw, float data_pitch)
{
    //爱来自GPT
    const uint8_t *p = (uint8_t *) &data_yaw;   //const uint8_t *p是指针指向的变量是常量，uint8_t * const p是指针是常量
    can_send_content_SMcomputer[0] = p[0];
    can_send_content_SMcomputer[1] = p[1];
    can_send_content_SMcomputer[2] = p[2];
    can_send_content_SMcomputer[3] = p[3];

    p = (uint8_t *)&data_pitch;
    can_send_content_SMcomputer[4] = p[0];
    can_send_content_SMcomputer[5] = p[1];
    can_send_content_SMcomputer[6] = p[2];
    can_send_content_SMcomputer[7] = p[3];
}


//------------------------------------------------------------------------------------------------------
// 函数简介     小电脑沟通-发报
// 使用示例     small_computer_connect_SendtoDOWN(&hcan1);
// 输入参数     hcan                    CAN句柄
// 返回参数     无
// 备注信息     无
//------------------------------------------------------------------------------------------------------
void small_computer_connect_Send(CAN_HandleTypeDef *hcan) {
    TxHeader_to_SMcomputer.StdId = 0x122;
    for (uint8_t i = 0; i < 10; i++) {                   //发信失败则再试9次
        if (HAL_CAN_IsTxMessagePending(hcan, Tx_mailbox_toSMcomputer)==RESET) {    //判断目标缓冲区是否为空
            if (HAL_OK == HAL_CAN_AddTxMessage(hcan,&TxHeader_to_SMcomputer,can_send_content_SMcomputer,&Tx_mailbox_toSMcomputer))
                break;
        }
    }
}

//------------------------------------------------------------------------------------------------------
// 函数简介         键鼠控制
// 使用示例         AWSD_Remote_Control()
// 参数介绍
// 返回参数         无
// 备注信息         仅使用键鼠控制云台
//------------------------------------------------------------------------------------------------------
void AWSD_Remote_Control(void) {

}